#include <tracker/tracker.h>


/**
 * 	The tracker location service provides real-time location estimates
 * 	to whatever node requires it. The request should be empty, and the
 * 	response takes the form of absolute coordinates with respect to the
 * 	starting position of the robot.
 **/
bool Tracker::location_service	(SLAM::Location::Request 	& req,
				 	 	 	 	 SLAM::Location::Response 	& res)
{
	if (req.resample) {
		filter_.estimatedPosition(&res.x, &res.y, &res.angle);
	} else {
		filter_.cachedEstimatedPosition(&res.x, &res.y, &res.angle);
	}

	res.x = current_x;
	res.y = current_y;
	res.angle = current_angle;

	return true;
}
